#include "cylinder.h"
#include "CSerialPort.h"
#include "global.h"

using namespace std;
typedef pcl::PointXYZ PointT;

void findcylinder_run()
{
	send_data[0] = 1;
	int times = 0;
	short pre_data[10][3];//0 x;  1 z;   2 size; 

							// Build a passthrough filter to remove spurious NaNs

	pass_plane.setInputCloud(cloud_filtered);
	pass_plane.setFilterFieldName("y");
	pass_plane.setFilterLimits(-0.35, 0.05);
	pass_plane.filter(*cloud_filtered_plane);
	
	sor_plane.setInputCloud(cloud_filtered_plane);
	sor_plane.setLeafSize(0.015f, 0.015f, 0.015f);
	sor_plane.filter(*cloud_filtered_plane);

	ne.setSearchMethod(tmp_tree);
	ne.setInputCloud(cloud_filtered_plane);
	ne.setKSearch(50);
	ne.compute(*cloud_normals);

	seg_cylinder.setOptimizeCoefficients(true);
	seg_cylinder.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	seg_cylinder.setNormalDistanceWeight(0.1);
	seg_cylinder.setMethodType(pcl::SAC_RANSAC);
	seg_cylinder.setMaxIterations(100);
	seg_cylinder.setDistanceThreshold(0.03);
	seg_cylinder.setInputCloud(cloud_filtered_plane);
	seg_cylinder.setInputNormals(cloud_normals);
	// Obtain the plane inliers and coefficients
	seg_cylinder.segment(*inliers_plane, *coefficients_plane);
	
	extract.setInputCloud(cloud_filtered_plane);
	extract.setIndices(inliers_plane);
	extract.setNegative(false);

	// Write the planar inliers to disk
	extract.filter(*cloud_plane);

	float a, b, c, d;
	short angle=0, distance=0;
	if (coefficients_plane->values.size() > 0)
	{
		all_cloud_cluster_cylinder->operator+=(*cloud_plane);
		a = coefficients_plane->values[0];
		b = coefficients_plane->values[1];
		c = coefficients_plane->values[2];
		d = coefficients_plane->values[3];
		angle = (atan2(c, a) * 180 / 3.14 - 90)*100;
		distance = (abs(d) / sqrt(a*a + b*b + c*c))*1000;
		cout << "角度: " << angle/100;
		cout << "    距离" << distance/ 10<<endl;
		send_data[5]=2;
	}



	sor_cylinder.setInputCloud(cloud_filtered);
	sor_cylinder.setLeafSize(0.004f, 0.008f, 0.005f);
	sor_cylinder.filter(*cloud_filtered_cylinder);

	pass_cylinder.setInputCloud(cloud_filtered_cylinder);
	pass_cylinder.setFilterFieldName("y");
	pass_cylinder.setFilterLimits(-0.15, 0.15);
	pass_cylinder.filter(*cloud_filtered_cylinder);

	//std::cerr << "PointCloud after filtering has: " << cloud_filtered_cylinder->points.size() << " data points." << std::endl;

	tree_cylinder->setInputCloud(cloud_filtered_cylinder);

	std::vector<pcl::PointIndices> cluster_indices;
	ec.setClusterTolerance(0.012); // 1.3cm
	ec.setMinClusterSize(500);
	ec.setMaxClusterSize(1300);
	ec.setSearchMethod(tree_cylinder);
	ec.setInputCloud(cloud_filtered_cylinder);
	ec.extract(cluster_indices);

	all_cloud_cluster_cylinder->resize(cloud_filtered_cylinder->size());
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
			cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
		feature_extractor.setInputCloud(cloud_cluster);
		feature_extractor.compute();

		pcl::PointXYZ min_point_AABB;
		pcl::PointXYZ max_point_AABB;
		feature_extractor.getAABB(min_point_AABB, max_point_AABB);

		if (z > 0.4)y_min = 0.295;
		else y_min = 0.28;
		if (((max_point_AABB.x - min_point_AABB.x)>0.06) && ((max_point_AABB.x - min_point_AABB.x)<0.115) && ((max_point_AABB.z - min_point_AABB.z)<0.11) && ((max_point_AABB.y - min_point_AABB.y)>y_min))
		{
			all_cloud_cluster_cylinder->operator+=(*cloud_cluster);
			pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
			pre_data[times][1] = min_point_AABB.z * 1000;
			pre_data[times][2] = cloud_cluster->size();
			times++;
			std::cout << max_point_AABB.x - min_point_AABB.x << "  " << max_point_AABB.z - min_point_AABB.z << "  " << max_point_AABB.y - min_point_AABB.y << "  " << ((max_point_AABB.x + min_point_AABB.x) / 2) * 100 << "  " << min_point_AABB.z * 100 << "        size: " << cloud_cluster->points.size() << std::endl;
		}
		//std::cout << "                       size: " << cloud_cluster->points.size() << " data points." << std::endl;

	}
	if (times == 0)
	{
		x = z = 0;
		send_data[0] = 0;
	}
	else if (times == 1)
		{
			x = pre_data[0][0];
			z = pre_data[0][1];
		}
		else
		{
			short temp_x, temp_z, temp_size, temp_min_size, max_n, min_n;
			temp_x = temp_z = temp_size = temp_min_size = max_n = min_n = 0;
			for (int i = 0; i < times; i++)
			{
				if (pre_data[i][2] > temp_size)
				{
					temp_min_size = temp_size;
					min_n = max_n;
					temp_size = pre_data[i][2];
					max_n = i;
				}
			}
			if (temp_size - temp_min_size > 100)
			{
				x = pre_data[max_n][0];
				z = pre_data[max_n][1];
			}
			else
			{
				if (abs(pre_data[max_n][0]) < abs(pre_data[min_n][0]))
				{
					x = pre_data[max_n][0];
					z = pre_data[max_n][1];
				}
				else
				{
					x = pre_data[min_n][0];
					z = pre_data[min_n][1];
				}
			}
	if(send_data[5]==2&&send_data[0]==1)send_data[0]=1;
	else send_data[0]=0;
	std::cout << x/10 << "  " << z/10<<endl;

	send_data[1] = x;
	send_data[2] = (x >> 8);
	send_data[3] = z;
	send_data[4] = (z >> 8);
	send_data[5] = angle;
	send_data[6] = (angle >> 8);
	send_data[7] = 0x5A;	
}
}
